﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_CreateLRM.cs
 * \brief	This file contains the function that creates the LowResolutionMap.
 *  
 * This file contains the implementation of the CreateLRM function. This function
 * creates a Low Resolution Map.
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.03
 * \date      2010-02-04
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-07-30 \n
 *      First implementation of the CreateLRM function.
 *      
 * - Version 0.02:   Antonio MATTA                          2009-09-01 \n
 *      CreateLRM function according to the new version.
 *      
 * - Version 0.03:   Antonio MATTA                          2009-09-01 \n
 *      CreateLRM function according to the new version.
 *      
 * - Version 0.04:   Antonio MATTA                          2009-01-27 \n
 *      LowResMap_Fusion function interface modified: now is receiving a list
 *      of Map Cells.
 *      
 * - Version 0.05:   Antonio MATTA                          2009-01-27 \n
 *      UpdatedCellList_Fusion function interface modified: now is receiving a list
 *      of Map Cells.
 *      
 * -Version 0.06:   Luis VALDES                             2010-04-27 \n
 *      Eliminated unused input argument in function LowResMap_Fusion.
 *      Overloaded function LowResMap_Fusion renamed to FusionLRMwithCellList
 *      to avoid confusion.
 *           
/* ######################################################################### */

using System;
using System.Collections.Generic;
using System.Text;
using System.Globalization;
using System.IO;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;

/*This is for using lists*/
using System.Collections;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_CreateLRM
     *
     *  \brief  This is the class of the Low Resolution Map creation functions.
     *
     * This class grabs the incoming data and returns a Low Resolution Map of the
     * robot's sorrounding.
     * 
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_CreateLRM
    {
        #region CreateHigh2LowMap
        /* ------------------------------------------------------------------------- */
        /**	\fn public static cl_Map2D CreateLRM( cl_Map2D HighResMap, cl_Map2D LowResMap )
         * 
         *  \brief From the information coming in the input classes, it generates a Low
         *  Resolution Map.
         *  
         *  \param[in] cl_Map2D HighResMap
         *  \param[in] cl_Map2D  LowResMap
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_Map2D CreateHigh2LowMap(cl_Map2D HighResMap, cl_Map2D LowResMapGlobal )
        {
            #region VARIABLES
            cl_Map2D     High2LowRes_Map = new cl_Map2D();
            ushort       us_Scale_local = 0;
            ushort       us_Scale_global = 0;
            ushort       us_Scale_low = 0;
            ushort       us_Nrows_local = 0;
            ushort       us_Ncols_local = 0;
            ushort       us_Nrows_low = 0;
            ushort       us_Ncols_low = 0;
            ushort       us_n = 0;
            int          i_Confidence_rate;
            #endregion

            /* We take the information available from the High Resolution Map and the
               global low resolution map.
               High2LowRes_Map represents the same area as HighResMap but with the same resolution of LowResMap*/
            us_Scale_local  = (HighResMap.us_Scale);
            us_Scale_global = (LowResMapGlobal.us_Scale);
            us_Nrows_local  = HighResMap.us_Nrows;
            us_Ncols_local  = HighResMap.us_Ncols;

            /*We define the parameters for the Low Resolution Map*/
            us_n         = (ushort)(M44_SensorFusionAndMappingService.i_MapRes*us_Scale_global / us_Scale_local);
            us_Nrows_low = (ushort)(us_Nrows_local / us_n);
            us_Ncols_low = (ushort)(us_Ncols_local / us_n);
            us_Scale_low = (us_Scale_global);

            High2LowRes_Map.Center = HighResMap.Center;
            High2LowRes_Map.us_Nrows = us_Nrows_low;
            High2LowRes_Map.us_Ncols = us_Ncols_low;
            High2LowRes_Map.us_Scale = us_Scale_low;
            High2LowRes_Map.MapCell = new cl_MapCell[High2LowRes_Map.us_Nrows, High2LowRes_Map.us_Ncols];

            /*The LowResMap Cell structure must be created with zeros*/
            for (int i = 0; i < High2LowRes_Map.us_Nrows; i++)
            {
                for (int j = 0; j < High2LowRes_Map.us_Ncols; j++)
                {
                    /*Order of parameters:
                     c8_CommQuality_local = 0;
                     c8_Confidence_local = 0;
                     c8_GPSQuality_local = 0;
                     f_Height_local = 0f;
                     c8_Occupation_local = 0;
                     f_Gradient_x_local = 0f;
                     f_Gradient_y_local = 0f;
                     c8_TractionEstimation_local = 0; */

                    High2LowRes_Map.MapCell[i, j] = new cl_MapCell(0, 0, 0, 0.0f, 0, 0f, 0f, 0);
                }
            }

            /*Fill the High2LowRes_Map with data coming from the LowResMapGlobal*/
            for (int I = 0; I < us_Nrows_low; I++)
            {
                for (int J = 0; J < us_Ncols_low; J++)
                {
                    i_Confidence_rate = 0;
                    float f_Occupation_temp = 0;
                    High2LowRes_Map.MapCell[I, J].c8_Confidence = 255;

                    for (int i = (I*us_n); i < ((I+1)*us_n); i++)
                    {  
                       for (int j = (J*us_n); j < ((J+1)*us_n); j++)
                       {   
                           if (HighResMap.MapCell[i, j].c8_Confidence < High2LowRes_Map.MapCell[I, J].c8_Confidence)
                           {
                               High2LowRes_Map.MapCell[I, J].c8_Confidence = HighResMap.MapCell[i, j].c8_Confidence;
                           }
                           else
                           {
                               //Nothing to do
                           }

                           f_Occupation_temp = (float)((f_Occupation_temp + HighResMap.MapCell[i, j].c8_Confidence * HighResMap.MapCell[i, j].c8_Occupation));
                           High2LowRes_Map.MapCell[I, J].f_Height = High2LowRes_Map.MapCell[I, J].f_Height + HighResMap.MapCell[i, j].c8_Confidence * HighResMap.MapCell[i, j].f_Height;
                           High2LowRes_Map.MapCell[I, J].f_Gradient_x = High2LowRes_Map.MapCell[I, J].f_Gradient_x + HighResMap.MapCell[i, j].c8_Confidence * HighResMap.MapCell[i, j].f_Gradient_x;
                           High2LowRes_Map.MapCell[I, J].f_Gradient_y = High2LowRes_Map.MapCell[I, J].f_Gradient_y + HighResMap.MapCell[i, j].c8_Confidence * HighResMap.MapCell[i, j].f_Gradient_y;
                           /*GPS Fusion*/

                           High2LowRes_Map.MapCell[I, J].c8_GPSQuality = Math.Max(High2LowRes_Map.MapCell[I, J].c8_GPSQuality, HighResMap.MapCell[i, j].c8_GPSQuality);
                           
                           /*Communication Quality fusion*/
                           High2LowRes_Map.MapCell[I, J].c8_CommQuality = (byte)(High2LowRes_Map.MapCell[I, J].c8_CommQuality + HighResMap.MapCell[i, j].c8_CommQuality);
                           i_Confidence_rate = (byte)((i_Confidence_rate + HighResMap.MapCell[i, j].c8_Confidence));
                       }
                    }
                    if (i_Confidence_rate != 0)
                    {
                        f_Occupation_temp = ((f_Occupation_temp / i_Confidence_rate));
                        High2LowRes_Map.MapCell[I, J].c8_Occupation = (byte)f_Occupation_temp;
                        High2LowRes_Map.MapCell[I, J].f_Height = High2LowRes_Map.MapCell[I, J].f_Height / i_Confidence_rate;
                        High2LowRes_Map.MapCell[I, J].f_Gradient_x = High2LowRes_Map.MapCell[I, J].f_Gradient_x / i_Confidence_rate;
                        High2LowRes_Map.MapCell[I, J].f_Gradient_y = High2LowRes_Map.MapCell[I, J].f_Gradient_y / i_Confidence_rate;



                        /*Communication layer fusion*/
                        High2LowRes_Map.MapCell[I, J].c8_CommQuality = High2LowRes_Map.MapCell[I, J].c8_CommQuality;
                        i_Confidence_rate = (byte)((i_Confidence_rate / (us_n * us_n)));
                        High2LowRes_Map.MapCell[I, J].c8_Confidence = (byte)i_Confidence_rate;
                    }
                    else
                    {
                        //Nothing to do
                    }

                }
            }
        
            return High2LowRes_Map;
        }
        #endregion CreateHigh2LowMap
        
        #region LowResMap_Fusion
        /* ------------------------------------------------------------------------- */
        /**	\fn public static cl_Map2D LowResMap_Fusion( cl_Map2D LowResMap, cl_Map2D High2LowRes_Map)
         * 
         *  \brief From the information coming in the input classes, it generates a Low
         *  Resolution Map.
         *  
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] cl_Map2D High2LowRes_Map
         *
         */
        /* ------------------------------------------------------------------------- */
        public static cl_Map2D LowResMap_Fusion( cl_Map2D LowResMap, cl_Map2D High2LowRes_Map )
        {
            cl_Map2D    LowResMap_aux = new cl_Map2D();
            double[]    td_CenterCoordinates = new double[2];
            int[]       ti_CenterCell = new int[2];
            int         i_iInit;
            int         i_iFinal;
            int         i_jInit;
            int         i_jFinal;

            /*It is computed the cell on the LowResMap of the High2LowRes_Map center*/
            td_CenterCoordinates[0] = High2LowRes_Map.Center.d_x;
            td_CenterCoordinates[1] = High2LowRes_Map.Center.d_y;

            ti_CenterCell = M44_BasicClass.Coordinates2Cell( LowResMap, td_CenterCoordinates);

            /*The area of the LowResMap to be merged with the new measures is defined*/
            i_iInit  = ti_CenterCell[0] - ( (High2LowRes_Map.us_Nrows / 2) - 1);
            i_iFinal = ti_CenterCell[0] + (High2LowRes_Map.us_Nrows / 2);
            i_jInit  = ti_CenterCell[1] - (High2LowRes_Map.us_Ncols / 2);
            i_jFinal = ti_CenterCell[1] + ( (High2LowRes_Map.us_Ncols / 2) - 1);

            /*The High2LowRes_Map is resized with the dimensions of LowResMap*/
            LowResMap_aux.us_Nrows = LowResMap.us_Nrows;
            LowResMap_aux.us_Ncols = LowResMap.us_Ncols;
            LowResMap_aux.us_Scale = LowResMap.us_Scale;
            LowResMap_aux.Center   = LowResMap.Center;

            LowResMap_aux.MapCell = new cl_MapCell[LowResMap_aux.us_Nrows, LowResMap_aux.us_Ncols];
            for (int i = 0; i < LowResMap_aux.us_Nrows; i++)
            {
                for (int j = 0; j < LowResMap_aux.us_Ncols; j++)
                {
                    LowResMap_aux.MapCell[i, j] = new cl_MapCell(0, 0, 0, 0.0f, 0, 0f, 0f, 0);
                }
            }

            /* Temporal aux_MapCell variable where the LowResMap Cell data is stored*/
            aux_MapCell aux_MapCell = new aux_MapCell(LowResMap_aux.us_Nrows, LowResMap_aux.us_Ncols);

            /* The LowResMap_aux is created with the information of High2LowRes_Map 
             * and the size of LowResMap filling the unknown information with zeros*/

            for (int i = i_iInit; i < i_iFinal; i++)
            {
                for (int j = i_jInit; j < i_jFinal; j++)
                {
                    LowResMap_aux.MapCell[i, j].c8_Confidence = (byte)(High2LowRes_Map.MapCell[(i - i_iInit + 1), (j - i_jInit + 1)].c8_Confidence);
                    LowResMap_aux.MapCell[i, j].c8_Occupation = (byte)(High2LowRes_Map.MapCell[(i - i_iInit + 1), (j - i_jInit + 1)].c8_Occupation);
                    LowResMap_aux.MapCell[i, j].f_Height = High2LowRes_Map.MapCell[(i - i_iInit + 1), (j - i_jInit + 1)].f_Height;
                    LowResMap_aux.MapCell[i, j].c8_CommQuality = High2LowRes_Map.MapCell[(i - i_iInit + 1), (j - i_jInit + 1)].c8_CommQuality;
                    LowResMap_aux.MapCell[i, j].c8_GPSQuality = High2LowRes_Map.MapCell[(i - i_iInit + 1), (j - i_jInit + 1)].c8_GPSQuality;
                    LowResMap_aux.MapCell[i, j].f_Gradient_x = High2LowRes_Map.MapCell[(i - i_iInit + 1), (j - i_jInit + 1)].f_Gradient_x;
                    LowResMap_aux.MapCell[i, j].f_Gradient_y = High2LowRes_Map.MapCell[(i - i_iInit + 1), (j - i_jInit + 1)].f_Gradient_y;
                    
                    /*We copy the result into an aux_MapCell structure*/
                    aux_MapCell.Confidence[i, j] = (byte)(LowResMap_aux.MapCell[i, j].c8_Confidence);
                    aux_MapCell.Occupation[i, j] = (byte)(LowResMap_aux.MapCell[i, j].c8_Occupation);
                    aux_MapCell.Measured_Heights[i,j] = LowResMap_aux.MapCell[i, j].f_Height;
                    aux_MapCell.Communication_Quality[i,j] = LowResMap_aux.MapCell[i, j].c8_CommQuality;
                    aux_MapCell.GPS_reception_quality[i,j] = LowResMap_aux.MapCell[i, j].c8_GPSQuality;
                    aux_MapCell.Measured_Gradient_X[i,j] = LowResMap_aux.MapCell[i, j].f_Gradient_x;
                    aux_MapCell.Measured_Gradient_Y[i, j] = LowResMap_aux.MapCell[i, j].f_Gradient_y;
                }
            }
            
            /*Now the Layers function is called to merge the old values with the new ones*/
            LowResMap = M44_FusionTemp2Local.LayersFusion( LowResMap, aux_MapCell );
            /*The layers wich no need a special fusion are copied*/
            for (int i = 0; i < LowResMap.us_Nrows; i++)
            {
                for (int j = 0; j < LowResMap.us_Ncols; j++)
                {
                    if( (LowResMap.MapCell[i, j].c8_GPSQuality != LowResMap_aux.MapCell[i, j].c8_GPSQuality)&&(LowResMap_aux.MapCell[i, j].c8_Confidence>0))
                    {
                        LowResMap.MapCell[i, j].c8_GPSQuality = (byte)LowResMap_aux.MapCell[i, j].c8_GPSQuality;
                       
                    }
                    if ((LowResMap.MapCell[i, j].c8_CommQuality != LowResMap_aux.MapCell[i, j].c8_CommQuality) && (LowResMap_aux.MapCell[i, j].c8_Confidence > 0))
                    {
                        LowResMap.MapCell[i, j].c8_CommQuality = (byte)LowResMap_aux.MapCell[i, j].c8_CommQuality;

                    }
                }
            }

            return LowResMap;
        }
        #endregion LowResMap_Fusion

        #region FusionLRMwithCellList
        /* ---------------------------------------------------------------------------------------------------- */
        /**	public static cl_Map2D FusionLRMwithCellList(cl_Map2D LowResMap, cl_UpdatedCells CellList)
         * 
         *  \brief Algorithm to merge the processed information present in the LowResMap and the
         *         incoming Updated Cell List.
         *  
         * 
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] cl_UpdatedCells CellList
         *
         */
        /* ----------------------------------------------------------------------------------------------------- */
        public static cl_Map2D FusionLRMwithCellList(cl_Map2D LowResMap, cl_UpdatedCells CellList)
        {
            float f_gradient_variation = (float)(M44_SensorFusionAndMappingService.i_MapRes * Math.PI / 180);  /*Absolut variation of the gradient measures*/
            float f_height_variation = 0.5f;                           /*Absolut variation of the height, distance in meters*/      
            float f_old_height;
            float f_new_height;
            float f_old_gradientX;
            float f_old_gradientY;
            float f_new_gradientX;
            float f_new_gradientY;
            int i_confidence_inf_limit;
            int i_confidence_sup_limit;
            byte c8_old_confidence;
            byte c8_new_confidence;
            byte c8_old_ocupation;
            byte c8_new_ocupation;
            byte c8_confidence_variation = 2;   /*The number of Quads necesary to be considered a great variation on
                                                 * confidence */
           
            /*It starts the merge*/

            for (int cont = 0; cont < CellList.us_NumCells; cont++)
            {
                cl_UpdatedCell tmpMapCell = new cl_UpdatedCell();
                tmpMapCell = CellList.UpdatedCell[cont];
              if (tmpMapCell != null)
              {

                int i = tmpMapCell.us_CellIndex[0];
                int j = tmpMapCell.us_CellIndex[1];

                if (tmpMapCell.MapCell.c8_Confidence == (byte)53)
                {
                  LowResMap.MapCell[i, j].c8_CommQuality = tmpMapCell.MapCell.c8_CommQuality;
                  LowResMap.MapCell[i, j].c8_Confidence = 1;
                  LowResMap.MapCell[i, j].c8_GPSQuality = tmpMapCell.MapCell.c8_GPSQuality;
                  LowResMap.MapCell[i, j].c8_Occupation = tmpMapCell.MapCell.c8_Occupation;
                  LowResMap.MapCell[i, j].c8_TractionEstimation = tmpMapCell.MapCell.c8_TractionEstimation;
                  LowResMap.MapCell[i, j].f_Gradient_x = tmpMapCell.MapCell.f_Gradient_x;
                  LowResMap.MapCell[i, j].f_Gradient_y = tmpMapCell.MapCell.f_Gradient_y;
                  LowResMap.MapCell[i, j].f_Height = tmpMapCell.MapCell.f_Height;
                }

                else
                {
                  if (tmpMapCell.MapCell.c8_Confidence > 0 && tmpMapCell.MapCell.c8_Confidence != (byte)53)
                  {
                    f_old_height = LowResMap.MapCell[i, j].f_Height;
                    f_new_height = tmpMapCell.MapCell.f_Height;
                    f_old_gradientX = LowResMap.MapCell[i, j].f_Gradient_x;
                    f_old_gradientY = LowResMap.MapCell[i, j].f_Gradient_y;
                    f_new_gradientX = tmpMapCell.MapCell.f_Gradient_x;
                    f_new_gradientY = tmpMapCell.MapCell.f_Gradient_y;

                    c8_old_confidence = LowResMap.MapCell[i, j].c8_Confidence;
                    c8_new_confidence = tmpMapCell.MapCell.c8_Confidence;

                    c8_old_ocupation = LowResMap.MapCell[i, j].c8_Occupation;
                    c8_new_ocupation = tmpMapCell.MapCell.c8_Occupation;

                    i_confidence_inf_limit = (int)(c8_old_confidence - c8_confidence_variation);
                    if (i_confidence_inf_limit < 0)
                    {
                      i_confidence_inf_limit = 0;
                    }

                    i_confidence_sup_limit = (int)(c8_old_confidence + c8_confidence_variation);
                    if (i_confidence_sup_limit > 1)
                    {
                      i_confidence_sup_limit = 1;
                    }

                    /*Now it begins comparing gradients  layers to fuse the
                    layers looking for changes into the measures.*/

                    /*Similar GradientX and  Similar GradientY*/
                    if ((M44_FusionTemp2Local.similar_gradient(f_new_gradientX, f_old_gradientX, f_gradient_variation) && M44_FusionTemp2Local.similar_gradient(f_new_gradientY, f_old_gradientY, f_gradient_variation)))
                    {
                      LowResMap.MapCell[i, j].f_Gradient_x = (f_old_gradientX * c8_old_confidence + f_new_gradientX * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                      LowResMap.MapCell[i, j].f_Gradient_y = (f_old_gradientY * c8_old_confidence + f_new_gradientY * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                      LowResMap.MapCell[i, j].f_Height = (f_old_height * c8_old_confidence + f_new_height * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                      LowResMap.MapCell[i, j].c8_Occupation = (byte)((c8_old_ocupation * c8_old_confidence + c8_new_ocupation * c8_new_confidence) / (c8_old_confidence + c8_new_confidence));
                      LowResMap.MapCell[i, j].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);
                    }
                    /*Diferent Heights or Diferent Gradients with new_confidence higher than old_confidence*/
                    else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                                (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                                (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                                (c8_new_confidence >= i_confidence_sup_limit))
                    {
                      LowResMap.MapCell[i, j].f_Height = f_new_height;
                      LowResMap.MapCell[i, j].f_Gradient_x = f_new_gradientX;
                      LowResMap.MapCell[i, j].f_Gradient_y = f_new_gradientY;
                      LowResMap.MapCell[i, j].c8_Occupation = c8_new_ocupation;
                      LowResMap.MapCell[i, j].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);
                    }
                    /*Different Heights or Diferent Gradients with new_confidence similar to old_confidence*/
                    else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                                (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                                (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                                (c8_new_confidence > i_confidence_inf_limit) && (c8_new_confidence < i_confidence_sup_limit))
                    {
                      LowResMap.MapCell[i, j].f_Height = f_new_height;
                      LowResMap.MapCell[i, j].f_Gradient_x = f_new_gradientX;
                      LowResMap.MapCell[i, j].f_Gradient_y = f_new_gradientY;
                      LowResMap.MapCell[i, j].c8_Occupation = c8_new_ocupation;
                      LowResMap.MapCell[i, j].c8_Confidence = (byte)(1);
                    }
                  }
                }
                }
            }
            
            return LowResMap;
        }
        #endregion FusionLRMwithCellList

        #region UpdatedCellList_Fusion
        /* ---------------------------------------------------------------------------------------------------- */
        /**	public static cl_UpdatedCells UpdatedCellList_Fusion(cl_Map2D LowResMap, cl_UpdatedCells CellList)
         * 
         *  \brief Algorithm to merge the processed information present in the LowResMap and the
         *         incoming Updated Cell List.
         *  
         * 
         *  \param[in] cl_Map2D LowResMap
         *  \param[in] cl_UpdatedCells CellList
         *  
         *  \param[out] cl_UpdatedCells CellList
         *
         */
        /* ----------------------------------------------------------------------------------------------------- */
        public static cl_UpdatedCells UpdatedCellList_Fusion(cl_Map2D LowResMap, cl_UpdatedCells CellList)
        {
            float f_gradient_variation = (float)(M44_SensorFusionAndMappingService.i_MapRes * Math.PI / 180);  /*Absolut variation of the gradient measures*/
            float f_height_variation = 0.5f;                           /*Absolut variation of the height, distance in meters*/
            float f_old_height;
            float f_new_height;
            float f_old_gradientX;
            float f_old_gradientY;
            float f_new_gradientX;
            float f_new_gradientY;
            int i_confidence_inf_limit;
            int i_confidence_sup_limit;
            byte c8_old_confidence;
            byte c8_new_confidence;
            byte c8_old_ocupation;
            byte c8_new_ocupation;
            byte c8_confidence_variation = 2;                          /*The number of Quads necesary to be considered a great variation on
                                                                        * confidence*/

            /*It starts the merge*/

            for (int cont = 0; cont < CellList.us_NumCells; cont++)
            {
                cl_UpdatedCell tmpMapCell = new cl_UpdatedCell();
                tmpMapCell = CellList.UpdatedCell[cont];

                int i = tmpMapCell.us_CellIndex[0];
                int j = tmpMapCell.us_CellIndex[1];

                if (tmpMapCell.MapCell.c8_Confidence > 0)
                {
                    f_old_height = LowResMap.MapCell[i, j].f_Height;
                    f_new_height = tmpMapCell.MapCell.f_Height;
                    f_old_gradientX = LowResMap.MapCell[i, j].f_Gradient_x;
                    f_old_gradientY = LowResMap.MapCell[i, j].f_Gradient_y;
                    f_new_gradientX = tmpMapCell.MapCell.f_Gradient_x;
                    f_new_gradientY = tmpMapCell.MapCell.f_Gradient_y;

                    c8_old_confidence = LowResMap.MapCell[i, j].c8_Confidence;
                    c8_new_confidence = tmpMapCell.MapCell.c8_Confidence;

                    c8_old_ocupation = LowResMap.MapCell[i, j].c8_Occupation;
                    c8_new_ocupation = tmpMapCell.MapCell.c8_Occupation;

                    i_confidence_inf_limit = (int)(c8_old_confidence - c8_confidence_variation);
                    if (i_confidence_inf_limit < 0)
                    {
                        i_confidence_inf_limit = 0;
                    }

                    i_confidence_sup_limit = (int)(c8_old_confidence + c8_confidence_variation);
                    if (i_confidence_sup_limit > 1)
                    {
                        i_confidence_sup_limit = 1;
                    }

                    /*Now it begins comparing gradients  layers to fuse the
                    layers looking for changes into the measures.*/

                    /*Similar GradientX and  Similar GradientY*/
                    if ((M44_FusionTemp2Local.similar_gradient(f_new_gradientX, f_old_gradientX, f_gradient_variation) && M44_FusionTemp2Local.similar_gradient(f_new_gradientY, f_old_gradientY, f_gradient_variation)))
                    {
                        LowResMap.MapCell[i, j].f_Gradient_x = (f_old_gradientX * c8_old_confidence + f_new_gradientX * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                        LowResMap.MapCell[i, j].f_Gradient_y = (f_old_gradientY * c8_old_confidence + f_new_gradientY * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                        LowResMap.MapCell[i, j].f_Height = (f_old_height * c8_old_confidence + f_new_height * c8_new_confidence) / (c8_old_confidence + c8_new_confidence);
                        LowResMap.MapCell[i, j].c8_Occupation = (byte)((c8_old_ocupation * c8_old_confidence + c8_new_ocupation * c8_new_confidence) / (c8_old_confidence + c8_new_confidence));
                        LowResMap.MapCell[i, j].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);
                    }

                    /*Diferent Heights or Diferent Gradients with new_confidence higher than old_confidence*/
                    else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                          (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                          (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                          (c8_new_confidence >= i_confidence_sup_limit))
                    {
                        LowResMap.MapCell[i, j].f_Height = f_new_height;
                        LowResMap.MapCell[i, j].f_Gradient_x = f_new_gradientX;
                        LowResMap.MapCell[i, j].f_Gradient_y = f_new_gradientY;
                        LowResMap.MapCell[i, j].c8_Occupation = c8_new_ocupation;
                        LowResMap.MapCell[i, j].c8_Confidence = Math.Max(c8_old_confidence, c8_new_confidence);
                    }

                    /*Different Heights or Diferent Gradients with new_confidence similar to old_confidence*/
                    else if (((f_old_height <= f_new_height - f_height_variation) || (f_old_height >= f_new_height + f_height_variation) ||
                          (f_old_gradientX <= f_new_gradientX - f_gradient_variation) || (f_old_gradientX >= f_new_gradientX + f_gradient_variation) ||
                          (f_old_gradientY <= f_new_gradientY - f_gradient_variation) || (f_old_gradientY >= f_new_gradientY + f_gradient_variation)) &&
                          (c8_new_confidence > i_confidence_inf_limit) && (c8_new_confidence < i_confidence_sup_limit))
                    {
                        LowResMap.MapCell[i, j].f_Height = f_new_height;
                        LowResMap.MapCell[i, j].f_Gradient_x = f_new_gradientX;
                        LowResMap.MapCell[i, j].f_Gradient_y = f_new_gradientY;
                        LowResMap.MapCell[i, j].c8_Occupation = c8_new_ocupation;
                        LowResMap.MapCell[i, j].c8_Confidence = (byte)(1);
                    }

                    /*Store merged cell in output structure*/
                    tmpMapCell.MapCell = LowResMap.MapCell[i, j];
                    CellList.UpdatedCell[cont] = tmpMapCell;
                }
            }

            return CellList;
        }
        #endregion UpdatedCellList_Fusion
    }
}

/* ------------------------------------------------------------------------- */
/*
 * RIGHT OF USE. This document may neither be passed on to third parties or
 * reproduced nor its contents utilized or divulged without the expressed
 * prior permission of the EUROPEAN DEFENCE AGENCY, or any national government
 * having rights on it. In case of contravention, the offender shall be
 * liable for damages.
 *
 * ATTENTION! DEFENCE MATERIAL. This document may contain data which is subject
 * to export control. For the export of this data an export license is
 * required.
 *
 * COMPANY PROPRIETARY. This document contains proprietary information and
 * may only be used by the recipient for the prescribed purposes and may
 * neither be reproduced in any form nor the document itself or its content
 * divulged to third parties without our expressed prior written permission.
 *
 * COPYRIGHT (C) Diehl BGT Defence GmbH & Co. KG; 2008; All rights reserved; 
 *
 * DISTRIBUTION AND USAGE RIGHTS: Restricted to the NETWORKED MULTI-ROBOT
 * SYSTEMS Project Consortium, participation governments, prime contractor,
 * subcontractors and vendors in accordance with contract / subcontract
 * clauses and / or other specified written agreements.
 */
/* ------------------------------------------------------------------------- */
